﻿using System.Collections.Generic;
using UnityEngine;
using UnityEngine.AI;

namespace UFrame
{
    /// <summary>
    /// 路径寻找
    /// </summary>
    public class NavPathControl
    {
        NavMeshPath mPath;

        public Vector3 mEndPos = Vector3.zero;
        private static List<Vector3> findPathPointList = new List<Vector3>(); // 寻路路径
        private static List<Vector3> findPathToEnd = new List<Vector3>(); // 结束部分
        private static List<Vector3> findPathOnPath = new List<Vector3>(); // 中间部分
        private static List<Vector3> findPathFromStart = new List<Vector3>(); // 开头部分

        public NavPathControl()
        {

        }

        public void ResetInfo()
        {
            mPath.ClearCorners();
        }

        public static Vector3 GetSamplePosition(Vector3 pos, float dis = 2f)
        {
            NavMeshHit hit;
            if (NavMesh.SamplePosition(pos, out hit, dis, NavMesh.AllAreas))
            {
                return hit.position;
            }

            return pos;
        }

        public static bool IsOnNavMesh(Vector3 pos, float dist = 3f)
        {
            NavMeshHit hit;
            return NavMesh.SamplePosition(pos, out hit, 3f, NavMesh.AllAreas);
        }


        public static Vector3 Raycast(Vector3 startPos, Vector3 endPos)
        {
            NavMeshHit hit;

            if (NavMesh.Raycast(startPos, endPos, out hit, NavMesh.AllAreas))
            {
                return hit.position;
            }

            return endPos;
        }

        public static Vector3 FindClosestEdge(Vector3 pos)
        {
            NavMeshHit hit;

            if (NavMesh.FindClosestEdge(pos, out hit, NavMesh.AllAreas))
            {
                return hit.position;
            }

            return pos;
        }

        public static Vector3 ThreeCheck(Vector3 startPos, Vector3 endPos)
        {
            endPos = Raycast(startPos, endPos);
            endPos = FindClosestEdge(endPos);
            endPos = GetSamplePosition(endPos, 4);
            return endPos;
        }

        /// <summary>
        /// 寻找路径
        /// </summary>
        /// <param name="startPos"></param>
        /// <param name="endPos"></param>
        /// <returns></returns>
        public static Vector3[] FindPath(Vector3 startPos, Vector3 endPos, float radius = 1f)
        {
            // 1. 先尝试寻路层
            // 2. 尝试行走层到寻路层
            // 3. 返回行走层的点
            findPathPointList.Clear();
            findPathOnPath.Clear();
            findPathToEnd.Clear();
            findPathFromStart.Clear();

            int findPathArea = 22;

            NavMeshPath path = new NavMeshPath();

            // 优先级1：纯寻路层
            if (NavMesh.CalculatePath(startPos, endPos, findPathArea, path) && path.corners.Length > 1)
            {
                return path.corners;
            }

            if (NavMesh.CalculatePath(startPos, endPos, NavMesh.AllAreas, path))
            {
                if (path.corners != null && path.corners.Length == 2)
                {
                    return path.corners;
                }
            }

            NavMeshHit hitStart;
            NavMeshHit hitEnd;
            NavMeshHit hitFixed;
            bool jumpto4 = false;

            // 优先级2：直接行走层
            if (NavMesh.SamplePosition(startPos, out hitStart, 100f, findPathArea) &&
                NavMesh.SamplePosition(endPos, out hitEnd, 100f, findPathArea))
            {
                var dir1 = (hitStart.position - startPos);
                var dir2 = (endPos - hitEnd.position);
                dir1.y = 0;
                dir2.y = 0;
                if (!dir1.Equals(Vector3.zero) && !dir2.Equals(Vector3.zero))
                {
                    dir1.Normalize();
                    dir2.Normalize();

                    NavMeshHit hitCheck;
                    if (Vector3.Angle(dir1, dir2) > 120f &&
                        !NavMesh.Raycast(startPos, endPos, out hitCheck, findPathArea))
                    {
                        jumpto4 = true;
                        Debug.Log("Jumpto4");
                    }
                }
            }

            // NavMesh.FindClosestEdge();

            if (!jumpto4 &&
                NavMesh.SamplePosition(startPos, out hitStart, 100f, findPathArea) &&
                NavMesh.SamplePosition(endPos, out hitEnd, 100f, findPathArea))
            {
                // 寻路层到终点
                if (NavMesh.CalculatePath(hitEnd.position, endPos, NavMesh.AllAreas, path))
                {
                    int clipPos = 0;
                    // 修正
                    if (path.corners.Length > 1)
                    {
                        NavMeshHit hitCheck;
                        // 筛选Path中打折点
                        for (int i = 1; i < path.corners.Length; ++i)
                        {
                            // 筛选在FindPath上的点
                            if (NavMesh.SamplePosition(path.corners[i], out hitCheck, 0.5f, findPathArea))
                            {
                                clipPos = i;
                                continue;
                            }
                            else
                            {
                                break;
                            }
                        }

                        for (int i = 1; i < path.corners.Length - 2; i++)
                        {
                            bool result = NavMesh.FindClosestEdge(path.corners[i], out hitFixed, NavMesh.AllAreas);
                            if (result && hitFixed.distance < radius)
                            {
                                path.corners[i] = hitFixed.position + hitFixed.normal * radius;
                            }
                        }
                    }

                    // 筛选Path中打折点
                    for (int i = clipPos + 1; i < path.corners.Length; ++i)
                    {
                        findPathToEnd.Add(path.corners[i]);
                    }
                }

                // 起始点到寻路层                    
                if (NavMesh.CalculatePath(startPos, hitStart.position, NavMesh.AllAreas, path))
                {
                    int clipPos = path.corners.Length - 1;
                    // 修正
                    if (path.corners.Length > 1)
                    {
                        NavMeshHit hitCheck;

                        // 筛选Path中打折点
                        for (int i = path.corners.Length - 1; i >= 1; --i)
                        {
                            // 筛选在FindPath上的点
                            if (NavMesh.SamplePosition(path.corners[i], out hitCheck, 0.5f, findPathArea))
                            {
                                clipPos = i;
                                continue;
                            }
                            else
                            {
                                break;
                            }
                        }


                        for (int i = 1; i < path.corners.Length - 2; i++)
                        {
                            bool result = NavMesh.FindClosestEdge(path.corners[i], out hitFixed, NavMesh.AllAreas);
                            if (result && hitFixed.distance < radius)
                            {
                                path.corners[i] = hitFixed.position + hitFixed.normal * radius;
                            }
                        }
                    }

                    // 切除多余的部分
                    for (int i = 0; i < clipPos; ++i)
                    {
                        findPathFromStart.Add(path.corners[i]);
                    }
                }

                // 寻路层
                if (NavMesh.CalculatePath(hitStart.position, hitEnd.position, findPathArea, path))
                {
                    foreach (var corner in path.corners)
                    {
                        findPathOnPath.Add(corner);
                    }
                }

                // 切除多余的路径点
                if (findPathToEnd.Count != 0)
                {
                    // 割除结尾部分
                    int pos = FindNearestIndex(findPathOnPath, findPathToEnd[0]);
                    for (int i = findPathOnPath.Count - 1; i >= pos + 1; --i)
                    {
                        findPathOnPath.RemoveAt(i);
                    }
                }

                if (findPathFromStart.Count != 0)
                {
                    // 割除结尾部分
                    int pos = FindNearestIndex(findPathOnPath, findPathFromStart[findPathFromStart.Count - 1]);
                    for (int i = pos - 1; i >= 0; --i)
                    {
                        findPathOnPath.RemoveAt(i);
                    }
                }

                // 拼接路径
                foreach (var v in findPathFromStart)
                {
                    findPathPointList.Add(v);
                }

                foreach (var v in findPathOnPath)
                {
                    findPathPointList.Add(v);
                }

                foreach (var v in findPathToEnd)
                {
                    findPathPointList.Add(v);
                }

                return findPathPointList.ToArray();
            }


            // 优先级4：纯行走层
            if (NavMesh.CalculatePath(startPos, endPos, NavMesh.AllAreas, path))
            {
                // 修正
                if (path.corners.Length > 1)
                {
                    for (int i = 1; i < path.corners.Length - 2; i++)
                    {
                        bool result = NavMesh.FindClosestEdge(path.corners[i], out hitFixed, NavMesh.AllAreas);
                        if (result && hitFixed.distance < radius)
                        {
                            path.corners[i] = hitFixed.position + hitFixed.normal * radius;
                        }
                    }
                }
            }

            return path.corners;
        }

        /// <summary>
        /// 寻找最靠近指定点的点
        /// </summary>
        /// <param name="list"></param>
        /// <param name="targetPos"></param>
        /// <returns></returns>
        static int FindNearestIndex(List<Vector3> list, Vector3 targetPos)
        {
            if (list == null || list.Count == 0)
            {
                return 0;
            }

            int pos = 0;
            float dist = Vector3.Distance(list[0], targetPos);
            for (int i = 1; i < list.Count; ++i)
            {
                float tmp = Vector3.Distance(list[i], targetPos);
                if (tmp < dist)
                {
                    pos = i;
                    dist = tmp;
                }
            }

            return pos;
        }


        public void UnInit()
        {
            mPath.ClearCorners();
        }

        public bool CheckPath(List<Vector3> path, Vector3 position)
        {
            int count = path.Count;
            Vector3 start = position;
            Vector3 end = path[0];
            for (int i = 0; i < count - 1; i++)
            {
                if (NavMesh.CalculatePath(start, end, 255, mPath) && mPath.status == NavMeshPathStatus.PathComplete)
                {
                    start = end;
                    end = path[i + 1];
                }
                else
                {
                    return false;
                }
            }

            return true;
        }

        public List<Vector3> FindPathPosition(Vector3 target, Vector3 position)
        {
            if (NavMesh.CalculatePath(position, target, 255, mPath) && mPath.status == NavMeshPathStatus.PathComplete)
            {
                return new List<Vector3>(mPath.corners);
            }

            return null;
        }
    }
}